﻿/*
 * Copyright © 2020, Wuxi Safover Intelligent Technology Co., Ltd. All rights reserved.
 * 
 * 无锡安永智能科技有限公司版权所有，未经许可，不得用于商业用途
 * 
 * Visit: https://www.safover.com/ 
 */

using System;
using System.Diagnostics;
using System.Linq;
using Safover.LidarCollector.Core;

namespace Rt.VTH50
{
    internal class VTH50RotaryTableCommand
    {
        internal static readonly int FIXED_PACKET_LENGTH = 7;

        public CommandType CommandType => (CommandType)CommandBytes[3];

        public byte[] CommandBytes { get; }

        public float Angle
        {
            get
            {
                float angle = 0;

                if (CommandType == CommandType.UpHorizontalAngle || CommandType == CommandType.HorizontalAngleSet)
                {
                    angle = BitConverter.ToUInt16(new byte[] { CommandBytes[5], CommandBytes[4] }, 0) / 100f;
                }
                else if (CommandType == CommandType.UpVerticalAngle || CommandType == CommandType.VerticalAngleSet)
                {
                    angle = (360 - BitConverter.ToUInt16(new byte[] { CommandBytes[5], CommandBytes[4] }, 0) / 100f) % 360;
                }

                angle = (float)Math.Round(angle, 2);

                return angle;
            }
        }

        public VTH50RotaryTableCommand(byte[] command) 
        {
            Debug.Assert(null != command && command.Length == FIXED_PACKET_LENGTH);

            CommandBytes = command;
        }

        public override string ToString()
        {
            return $"{CommandType}";
        }
    }

    internal class VTH50RotaryTableCommandBuilder
    {
        private static string Name => "VTH50RotaryTableCommandBuilder";

        private const byte head = 0xFF;
        private const byte address = 0x01;
        private const byte empty = 0x00;

        public static VTH50RotaryTableCommand BuildStopCommand()
        {
            return new VTH50RotaryTableCommand(new byte[] { 0xFF, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01 });
        }

        public static VTH50RotaryTableCommand BuildCommand(CommandType type, float angle)
        {
            return Build(type, angle);
        }

        private static VTH50RotaryTableCommand Build(CommandType type, float angle) 
        {
            byte intruction = (byte)type;
            byte[] command = new byte[] { head, address, empty, intruction, empty, empty, empty };

            switch (type)
            {
                case CommandType.Stop:
                    return BuildStopCommand();
                case CommandType.VerticalAngleQuery:
                case CommandType.HorizontalAngleQuery:
                    break;
                case CommandType.VerticalAngleSet:
                    {
                        var value = (ushort)Math.Round((360 - angle) * 100f);
                        byte[] angleBytes = BitConverter.GetBytes(value);

                        command[4] = angleBytes[1];
                        command[5] = angleBytes[0];
                    }
                    break;
                case CommandType.HorizontalAngleSet:
                    {

                        var value = (ushort)Math.Round(angle * 100f);
                        byte[] angleBytes = BitConverter.GetBytes(value);

                        command[4] = angleBytes[1];
                        command[5] = angleBytes[0];
                    }
                    break;
                case CommandType.UpVerticalAngle:
                case CommandType.UpHorizontalAngle:
                default:
                    throw new InvalidOperationException($"{Name}: cannot build down-command type={type}; command=[{SerialPortDevice.ToHexString(command)}]");
            }

            command = AddCheck(command).ToArray();

            return new VTH50RotaryTableCommand(command);
        }

        internal static VTH50RotaryTableCommand BuildUpPacket(byte[] buffer)
        {
            CommandType type = (CommandType)buffer[3];

            switch (type)
            {
                case CommandType.UpVerticalAngle:
                case CommandType.UpHorizontalAngle:
                    break;
                case CommandType.Stop:
                    {
                        return null;
                    }
                default:
                    throw new InvalidOperationException($"{Name}: cannot build up-command type={type}. command=[{SerialPortDevice.ToHexString(buffer)}].");
            }

            return new VTH50RotaryTableCommand(buffer);
        }

        private static byte[] AddCheck(byte[] command)
        {
            byte check = 0x00;

            for (int i = 1; i < command.Length - 1; i++)
            {
                check += command[i];
            }

            check &= head;

            command[command.Length - 1] = check;

            return command;
        }
    }
}
